#include "ucontext.h"

#define ADDRESS_STACK_SIZE 32
#define IMEM_STACK_SIZE    16
#define OMEM_STACK_SIZE    1024
#define DMEM_STACK_SIZE    1024
#define MAX_QUEUES         16
#define MAX_TASKS          16
#define MAX_BUF_DATA       128
#define CHUNK_SIZE         1
#define MAX_CHUNKS         (MAX_BUF_DATA/CHUNK_SIZE)

#define ENOMEM             22

typedef struct task_s
{
  int id, priority;
  ucontext_t context;
} task_t;

typedef uchar task_queue_t;

typedef struct buffer_s
{
  sep uchar *rp, *wp, *stop;
  sep uchar *data;
  task_queue_t readq, writeq;
} buffer_t;

/*typedef struct buffer_s
{
  int rp, wp, size;
  sep uchar *data;
  task_queue_t readq, writeq;
} buffer_t;
*/

typedef struct buffer_chunk_s
{
  int rp, wp;
} buffer_chunk_t;

typedef struct buffer_c_s
{
  int rp, wp, size;
  buffer_chunk_t chunk;
  sep uchar *data[MAX_CHUNKS];
  task_queue_t readq, writeq;
} buffer_c_t;

/* Extern definitions we need because we're using macros */
extern uint _res;
extern task_t *currenttask, *oldtask;
extern task_queue_t run_queue;
extern ucontext_t maincontext;
extern sep uchar queuedata[MAX_QUEUES];
extern task_t tasks[MAX_TASKS];

/* OS functions */
void boot(void);
int kernel(void);

/* Task management functions */
int spawn(int id, void (*entry)(void*), void *arg);

#define _nexttask()                                \
  (oldtask=currenttask,                            \
  currenttask=qget(&run_queue),                    \
  swapcontext(&oldtask->context, &currenttask->context))

#define yield()                                    \
  (qadd(&run_queue, currenttask),                  \
  _nexttask())

#define block(queue)                               \
  (qadd(queue, currenttask),                       \
  _nexttask())

#define unblock(queue)                             \
  qmove(queue, &run_queue)

#define leave()                                    \
  (currenttask->id = -1,                           \
   _nexttask())

/* Task queue handling functions */
int qinit(task_queue_t *queue);

#define qadd(queue, task)                          \
  queuedata[*(queue)]:[(task)->id:] = 1

#define qdel(queue, task)                          \
  queuedata[*(queue)]:[(task)->id:] = 0

#define qget(queue)                                \
  (getleftsep(queuedata[*(queue)], PENUM, &_res) ? \
    (qdel(queue, &tasks[_res]), &tasks[_res]) :    \
    NULL)

#define qmove(src, dst)                            \
  (queuedata[*(dst)] |= queuedata[*(src)],         \
   queuedata[*(src)] = 0)

/* Buffer handling functions */
int bufinit(buffer_t *buf, int size);

#define bufread(buf, mydata)                       \
do {                                               \
  if ((buf)->rp == (buf)->wp)                      \
    block(&(buf)->writeq);                         \
                                                   \
  *(mydata) = *((buf)->rp);                        \
                                                   \
  unblock(&((buf)->readq));                        \
                                                   \
  (buf)->rp++;                                     \
  if ((buf)->rp == (buf)->stop)                    \
    (buf)->rp = (buf)->data;                       \
} while (0)

#define bufwrite(buf, mydata)                      \
do {                                               \
  sep uchar * wp = (buf)->wp + 1;                  \
  if (wp == (buf)->stop)                           \
    wp = (buf)->data;                              \
                                                   \
  if ((buf)->rp == wp)                             \
    block(&((buf)->readq));                        \
                                                   \
  *((buf)->wp) = *(mydata);                        \
                                                   \
  (buf)->wp = wp;                                  \
                                                   \
  unblock(&((buf)->writeq));                       \
} while (0)

/*
#define bufwrite(buf, mydata)                      \
do {                                               \
  sep uchar *wp = (buf)->wp + 1;                   \
  sep uchar *rp = (buf)->rp;                       \
  sep uchar *bufdata = (buf)->data;                \
  if (wp == (buf)->stop)                           \
    wp = bufdata;                                  \
                                                   \
  if (rp == wp)                                    \
    block(&((buf)->readq));                        \
                                                   \
  *((buf)->wp) = *(mydata);                        \
                                                   \
  (buf)->wp = wp;                                  \
                                                   \
  unblock(&((buf)->writeq));                       \
} while (0)
*/


/*
#define bufread(buf, mydata)                       \
do {                                               \
  if ((buf)->rp == (buf)->wp)                      \
    block(&(buf)->writeq);                         \
                                                   \
  *(mydata) = (buf)->data[(buf)->rp];              \
                                                   \
  unblock(&((buf)->readq));                        \
                                                   \
  (buf)->rp++;                                     \
  if ((buf)->rp == (buf)->size)                    \
    (buf)->rp = 0;                                 \
} while (0)

#define bufwrite(buf, mydata)                      \
do {                                               \
  int wp = (buf)->wp + 1;                          \
  if (wp == (buf)->size)                           \
    wp = 0;                                        \
                                                   \
  if ((buf)->rp == wp)                             \
    block(&((buf)->readq));                        \
                                                   \
  (buf)->data[(buf)->wp] = *(mydata);              \
                                                   \
  (buf)->wp = wp;                                  \
                                                   \
  unblock(&((buf)->writeq));                       \
} while (0)
*/

int bufinit_c(buffer_c_t *buf, int size);

#define bufread_c(buf, mydata)                     \
do {                                               \
  if ((buf)->chunk.rp==CHUNK_SIZE)                 \
  {                                                \
    (buf)->rp++;                                   \
    if ((buf)->rp == (buf)->size)                  \
      (buf)->rp = 0;                               \
    (buf)->chunk.rp = 1;                           \
                                                   \
    unblock(&((buf)->readq));                      \
                                                   \
    if ((buf)->rp == (buf)->wp)                    \
      block(&(buf)->writeq);                       \
                                                   \
    *(mydata) = (buf)->data[(buf)->rp][0];         \
  }                                                \
  else                                             \
  {                                                \
    *(mydata) =                                    \
      (buf)->data[(buf)->rp][(buf)->chunk.rp++];   \
  }                                                \
} while (0)

#define bufwrite_c(buf, mydata)                    \
do {                                               \
  if ((buf)->chunk.wp == CHUNK_SIZE)               \
  {                                                \
    int newwp = (buf)->wp + 1;                     \
    if (newwp == (buf)->size)                      \
      newwp = 0;                                   \
    (buf)->chunk.wp = 1;                           \
                                                   \
    if ((buf)->rp == newwp)                        \
      block(&((buf)->readq));                      \
                                                   \
    (buf)->data[(buf)->wp][0] = *(mydata);         \
                                                   \
    (buf)->wp = newwp;                             \
                                                   \
    unblock(&((buf)->writeq));                     \
  }                                                \
  else                                             \
  {                                                \
    (buf)->data[(buf)->wp][(buf)->chunk.wp++] =    \
      *(mydata);                                   \
  }                                                \
} while (0)
